function [position] = ServoPosition(id)
%{

Returns the current position of the servo with the given id

%}
    
    
%     addpath('C:\Users\Pulith Widana\Documents\MATLAB\Dynamixel-AX12-Matlab');
%     addpath('C:\Users\Pulith Widana\Documents\MATLAB\Dynamixel-AX12-Matlab\import');
%     addpath('C:\Users\Pulith Widana\Documents\MATLAB\Dynamixel-AX12-Matlab\bin');
%     
%     
%   
%     Load the dynamixel library
    loadlibrary('dynamixel','dynamixel.h');
    DEFAULT_PORTNUM = SetPort;  % COM
    DEFAULT_BAUDNUM = 1;  % 1Mbps
    
    
    response = calllib('dynamixel','dxl_initialize',DEFAULT_PORTNUM ,DEFAULT_BAUDNUM);
    

    pause on
    
    %There should only be 2 Dynamixels total
    numberOfDynamixels = length(id);
    

    
    if response == 1
        
        
        %Broadcast ID
        calllib('dynamixel','dxl_set_txpacket_id', id);
        
        %Length is 14
        %That handles position and speed for two dynamixels
        calllib('dynamixel','dxl_set_txpacket_length',3);
        
        %Write instruction
        calllib('dynamixel','dxl_set_txpacket_instruction',2);
        
        %Starting address (goal position)
        % calllib('dynamixel','dxl_set_txpacket_parameter',0, 36);
        
        %length of data to write to each dynamixel
        %We're writing position and speed = 4 bytes
        % calllib('dynamixel','dxl_set_txpacket_parameter',1, 2);
        
        %transmit
        calllib('dynamixel','dxl_tx_packet');
        
        pause(.1);
     
        calllib('dynamixel','dxl_rx_packet');
        len = calllib('dynamixel','dxl_get_rxpacket_length');

        % asking what the present position is and assiging to pos
        lowByte = calllib('dynamixel','dxl_get_rxpacket_parameter', 36);
        highByte = calllib('dynamixel','dxl_get_rxpacket_parameter', 37);
        position = calllib('dynamixel','dxl_makeword', lowByte, highByte);
        
       
    else
        disp('Failed to open USB2Dynamixel!');
    end

    calllib('dynamixel','dxl_terminate');
    
    unloadlibrary('dynamixel');
    
    